# encoding=utf-8
import math
import time

from CacheDef import *
from MPU6050Api import *


class MPU6050CalApi:
    def __init__(self, mpu6050api: MPU6050Api):
        self.mpu6050api = mpu6050api

        # ====一下三个定义了陀螺仪的偏差===========
        self.Gx_offset = -3.06
        self.Gy_offset = 1.01
        self.Gz_offset = -0.88

        self.Gx = 0.0
        self.Gy = 0.0
        self.Gz = 0.0

        self.first_flag = True
        self.start_time = self.get_ms_time()

    def get_ms_time(self):
        return int(round(time.time() * 1000))

    def calculate(self, sleep_time: int):
        ax = self.mpu6050api.read_data(ACCEL_XOUT_H)
        ay = self.mpu6050api.read_data(ACCEL_YOUT_H)
        az = self.mpu6050api.read_data(ACCEL_ZOUT_H)
        gx = self.mpu6050api.read_data(GYRO_XOUT_H)
        gy = self.mpu6050api.read_data(GYRO_YOUT_H)
        gz = self.mpu6050api.read_data(GYRO_ZOUT_H)

        # acceleration grade
        Ax = ax / 16384.00
        Ay = ay / 16384.00
        Az = az / 16384.00

        # convert to angle
        Angel_accX = math.atan(Ax / math.sqrt(Az * Az + Ay * Ay)) * 180 / 3.14
        Angel_accY = math.atan(Ay / math.sqrt(Ax * Ax + Az * Az)) * 180 / 3.14
        Angel_accZ = math.atan(Az / math.sqrt(Ax * Ax + Ay * Ay)) * 180 / 3.14

        # angle acceleration grade
        ggx = gx / 131.00
        ggy = gy / 131.00
        ggz = gz / 131.00

        # integration
        now_time = self.get_ms_time()
        if not self.first_flag:
            time_span = now_time - self.start_time - (sleep_time * 1000)
        else:
            time_span = now_time - self.start_time
            self.first_flag = False

        self.Gx = self.Gx + (ggx - self.Gx_offset) * time_span / 1000
        self.Gy = self.Gy + (ggy - self.Gy_offset) * time_span / 1000
        self.Gz = self.Gz + (ggz - self.Gz_offset) * time_span / 1000

        self.start_time = now_time

        # output
        return [Angel_accX, Angel_accY, Angel_accZ], [self.Gx, self.Gy, self.Gz]